/*
 * util.h
 */

//
// Author: Gerben de Vries
// Date: Monday 14 January 2008
//
// Description: mml utility functions
//

#ifndef __mml_util_h__
#define __mml_util_h__

#define _USE_MATH_DEFINES
#include <math.h>
#include <exception>

// Two times pi:
#ifndef M_2PI
#define M_2PI (2.0 * M_PI)
#endif

// Number of degrees in one radian:
#ifndef M_DEG_RAD
#define M_DEG_RAD (180.0 / M_PI)
#endif

// Number of radians in one degree:
#ifndef M_RAD_DEG
#define M_RAD_DEG (M_PI / 180.0)
#endif

#include <iostream>
#include <float.h>

#include <mml/api.h>

#define M_INF (2.0 * DBL_MAX)


namespace mml
{
  /**
  \class NotBracketed mml/Function.h
  \brief Function not bracketed exception
  \ingroup mmlfunc
  */
  class NotBracketed: public std::exception
  {};

  /** Exception for mismatching sizes in linear algebra operations. */
  class SizeMismatch: public std::exception
  {};

  /** Exception for out of bounds index. */
  class IndexOutOfBounds: public std::exception
  {};

  /** Exception for division by zero. */
  class DivisionByZero: public std::exception
  {};

  /** Exception for lack of orthonormality. */
  class OrthonormalityDiscrepancy: public std::exception
  {};

  /** Small double. */
  double MMLAPI eps(double const& x);

  /** Small float. */
  float MMLAPI eps(float const& x);

  /** Round a number towards nearest integer. */
  template <typename Type>
  Type round(Type x)
  {
    return floor(x + Type(0.5f));
  }

  /** Solve a square, non-singular system by Gaussian elimination. */
  template <typename Type>
  void gauss(int n, Type* A, Type* b);

  /** Solve a system by Gram-Schmidt orthogonalization. */
  template <typename Type>
  void gramschmidt(int m, int n, Type* A, Type* b, Type* x, Type const& tol = eps(Type(100.0f)));

  /** One-dimensional rootfinder. */
  MMLAPI double secant(double (*f)(double const& x, void* userData),
                       double a,
                       double b,
                       double fa,
                       double fb,
                       double const& tol = 1.0e-3,
                       void* userData = 0);

  /** Multidimensional rootfinder. */
  template <typename Type>
  int broyden(int m, int n,
              Type* x0,
              Type* y0,
              void (*f)(Type const* x, Type* y, void* userData),
              Type const& tol = Type(1.0e-3f),
              void* userData = 0,
              int maxEval = 40);

  /** Multidimensional minimization. */
  MMLAPI double nelder(int n,
                       double* x,
                       double (*f)(double const* x, void* userData),
                       double const& h = 0.1,
                       double const& tol = 1.0e-4,
                       void* userData = 0);

  /** Implicit ODE solver. */
  template <typename Type>
  void impEuler(int n,  // state vector length
                void (*f)(Type const& t, Type const* y, Type* dy, void* userData),  // rhs function
                Type const& t0,  // interval start
                Type const& t1,  // interval end
                Type const* y0,  // initial state vector
                Type const* f0,  // initial derivative vector
                Type* y1,  // solution vector
                Type* err,  // estimated error
                void* userData = 0);

  /** ODE solver. */
  template <typename Type>
  void euler(int n,  // state vector length
             void (*f)(Type const& t, Type const* y, Type* dy, void* userData),  // rhs function
             Type const& t0,  // interval start
             Type const& t1,  // interval end
             Type const* y0,  // initial state vector
             Type const* f0,  // initial derivative vector
             Type* y1,  // solution vector
             Type* err,  // estimated error
             void* userData = 0);

  /** ODE solver. */
  template <typename Type>
  void rk2(int n,  // state vector length
           void (*f)(Type const& t, Type const* y, Type* dy, void* userData),  // rhs function
           Type const& t0,  // interval start
           Type const& t1,  // interval end
           Type const* y0,  // initial state vector
           Type const* f0,  // initial derivative vector
           Type* y1,  // solution vector
           Type* err,  // estimated error in y1
           void* userData = 0);

  /** ODE solver. */
  template <typename Type>
  void rk4(int n,  // state vector length
           void (*f)(Type const& t, Type const* y, Type* dy, void* userData),  // rhs function
           Type const& t0,  // interval start
           Type const& t1,  // interval end
           Type const* y0,  // initial state vector
           Type const* f0,  // initial derivative vector
           Type* y1,  // solution vector
           Type* err,  // estimated error in y1
           void* userData = 0);

  /** Higher order ODE solver. */
  template <typename Type>
  void cashkarp(int n,  // state vector length
                void (*f)(Type const& t, Type const* y, Type* dy, void* userData),  // rhs function
                Type const& t0,  // interval start
                Type const& t1,  // interval end
                Type const* y0,  // initial state vector
                Type const* f0,  // initial derivative vector
                Type* y1,  // solution vector
                Type* err,  // estimated error in y1
                void* userData = 0);

  /** Minimum of two numbers. */
  template <typename Type>
  Type const& min(Type const& a, Type const& b)
  {
    return (a < b ? a : b);
  }

  /** Maximum of two numbers. */
  template <typename Type>
  Type const& max(Type const& a, Type const& b)
  {
    return (a > b ? a : b);
  }

  /** Sign. */
  template <typename Type>
  Type sign(Type const& x)
  {
    if (x > Type(0.0f)) {
      return Type(1.0f);
    }
    else if (x < Type(0.0f)) {
      return Type(-1.0f);
    }
    return Type(0.0f);
  }

  /** Normalize an angle into the range [0, 2 * pi) */
  template <typename Type>
  Type normalizeAnglePositive(Type const& angle)
  {
    return angle - floor(angle * Type(0.5 / M_PI)) * Type(M_2PI);
  }

  /** Normalize an angle into the range (-pi, pi] */
  template <typename Type>
  Type normalizeAngleBalanced(Type const& angle)
  {
    return angle - round(angle * Type(0.5 / M_PI)) * Type(M_2PI);
  }

  /** Integer powers. */
  double MMLAPI pow(double const& x, int n);
}  // end of namespace mml

#endif
